#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <cmath>

int main()
{
    tf::Vector3 op(0,0,0);
    tf::Vector3 OA(1,0,0);
    tf::Vector3 OC(0,1,0);
    tf::Vector3 pa(1,1,0);
    tf::Vector3 A(0,1,0); // point a's direction
    double d = 2;   // translate distance
    tf::Vector3 B(-1,0,0); // translate direction
    std::cout << "angle: " << A.angle(B) << "\n";
    std::cout << "angle: " << B.angle(A) << "\n";
    std::cout << "A length: " << A.length() << "\n";
    tf::Vector3 A2 = 2*A;
    std::cout << "A2 length: " << A2.length() << "\n";
    tf::Quaternion qa(tf::Vector3(0,0,1),1.57);
    tf::Quaternion qb(tf::Vector3(0,0,1),3.14);
    tf::Quaternion qc(tf::Vector3(0,0,1),0.52);
    tf::Quaternion qd(tf::Vector3(0,0,1),0.785);
    tf::Quaternion qo(tf::Vector3(0,0,1),0);
    tf::Quaternion qr = qa+qd;
    tf::Vector3 trans(1.414,1.414,0);
    tf::Transform transform1(qo, tf::Vector3(0,0,0));
    tf::Transform transform2(qo, tf::Vector3(1,1,0));
    tf::Transform transform3(qd, tf::Vector3(0,0,0));
    tf::Transform transform4(qa, tf::Vector3(0,0,0));
    tf::Transform tr1 = transform3 * transform1;
    tf::Transform tr2 = transform3 * transform4;
    tf::Vector3 pb  = transform1 * pa;
    tf::Vector3 pb2 = transform2 * pa;
    tf::Vector3 pb3 = transform3 * pa;
    tf::Vector3 pb4 = transform4 * pa;
    //根据结果可以判断，transform的意思是：pa这个点先按Quaternion进行旋转，然后按Vector3平移
    //这里的平移是对应坐标轴上的平移，直接相加减
    // std::cout << pb.x() << ", " << pb.y() << ", "<< pb.z() << "\n";
    // std::cout << pb2.x() << ", " << pb2.y() << ", "<< pb2.z() << "\n";
    // std::cout << pb3.x() << ", " << pb3.y() << ", "<< pb3.z() << "\n";
    // std::cout << pb4.x() << ", " << pb4.y() << ", "<< pb4.z() << "\n";
    // std::cout << qr.getAngle() << ", " << (qr/2).getAngle() << "\n";
    // std::cout <<"qa.getAngle: " << qa.getAngle() << ", " << qr.getAngle() << "\n";
    // std::cout <<"qo: " << qo.x() << ", " << qo.y() << ", " << qo.z() << ", " << qo.w() << "\n";
    // std::cout <<"qa: " << qa.x() << ", " << qa.y() << ", " << qa.z() << ", " << qa.w() << "\n";
    // std::cout <<"qb: " << qb.x() << ", " << qb.y() << ", " << qb.z() << ", " << qb.w() << "\n";
    // std::cout <<"qc: " << qc.x() << ", " << qc.y() << ", " << qc.z() << ", " << qc.w() << "\n";
    // std::cout <<"qd: " << qd.x() << ", " << qd.y() << ", " << qd.z() << ", " << qd.w() << "\n";
    // std::cout <<"qr: " << qr.x() << ", " << qr.y() << ", " << qr.z() << ", " << qr.w() << "\n";
    // std::cout <<"qr/2: " << (qr/2).x() << ", " << (qr/2).y() << ", " << (qr/2).z() << ", " << (qr/2).w() << "\n";
    // std::cout <<"tr1,tr2: " << tr1.getRotation().getAngle() << ", " << tr2.getRotation().getAngle() << "\n";
    std::cout <<"pa angle: " << tfAcos(OA.dot(pa)/(OA.length()*pa.length())) << "\n";
    return 0;
}